function [loc,vel]=orb2state2(a,e,i,theta,omegab,u,trueAnom)
% [loc,vel]=orb2state2(a,e,i,theta,omegab,u,trueAnom)
%	a       : Semi-major axis
%	e       : Eccentricity
%	i       : Inclination
%	theta   : Longitude of ascending node
%	omegab  : Longitude of periapsis
%	u       : Standard gravitational parameter
%	trueAnom: True anomaly

%% Convert Angles
theta=deg2rad(theta);
omegab=deg2rad(omegab);
i=deg2rad(i);
trueAnom=mod(deg2rad(trueAnom),2*pi);

% Pseudocode
%  * calc nodal vector, n
%  * calc angular momentum vector, h
%  * calc position vector
%  *  calc argument of position
%  *  calc direction of position vector
%  *  calc length of position vector
%  * calc velocity vector
%  *  calculate magnitude of velocity vector
%  *  calc components of velocity vector perpendicular and parallel to radius vector
%  *  add to get velocity vector */

%% Calculate Position
if(e==1)
	error('no support for e=1 at this time');
end

% 	/* calc nodal vector */
% 	n = kostConstructv(cos(elements->theta),sin(elements->theta),0.0);
	n=[cos(theta),sin(theta),0];
	
% 	/* equatorial north vector */
% 	north = kostConstructv(0.0,0.0,1.0);
	north = [0 0 1];
	
% 	/* calc angular momentum vector, h */
% 	/* projection of h in ecliptic (xy) plane */
% 	h = kostCrossProductvv(&n, &north);
% 	h = kostMulrv( sin(elements->i), &h);
	h=cross(n,north).*i;
	
% 	/* elevation of h */
% 	h.z = cos(elements->i);
% 	h = kostNormalv(&h);
	h(3) = cos(i);
	h=h/norm(h);
	
% 	/* calc magnitude of h */
% 	/* calc radius and velocity at periapsis */
% 	if (e < 1.0) /* elliptical orbit */
% 	{
% 		rPe = elements->a * (1.0 - e * e) / (1.0 + e);
	rPe=a * (1 - e^2) / (1 + e);
% 		vPe = sqrt(mu*(2.0/rPe - 1.0/elements->a));
	vPe=sqrt(u * (2/rPe - 1/a));
% 	}
% 	else /* hyperbolic orbit */
% 	{
% 		rPe = fabs(elements->a) * (e * e - 1.0) / (1.0 + e);
% 		vPe = sqrt(mu*(2.0/rPe + 1.0/fabs(elements->a)));
	%TODO
% 	}

% 	/* calc h */
% 	h = kostMulrv(rPe*vPe,&h);
	h = h .* (rPe*vPe);
% 	/* calc position vector */
% 	/* calc argument of position */
% 	argPos = elements->omegab - elements->theta + trueAnomaly;
	argPos = omegab - theta + trueAnom;
	
% 	/*
% 	calc direction of position vector:
% 	r/|r| = sin(ArgPos) * ((h / |h|) x n) + cos(argPos) * n
% 	*/
% 	tmpv = kostMulrv(cos(argPos), &n);
	tmpv = cos(argPos).*n;
% 	state->pos = kostNormalv(&h);
	pos=h./norm(h);
% 	state->pos = kostCrossProductvv(&state->pos,&n);
	pos=cross(pos,n);
% 	state->pos = kostMulrv(sin(argPos), &(state->pos));
	pos=sin(argPos)*pos;
% 	state->pos = kostAddvv(&(state->pos), &tmpv);
	pos=pos+tmpv;

	
% 	/* calc length of position vector */
% 	if (e < 1.0) /* elliptical orbit */
% 		state->pos = kostMulrv(elements->a * (1.0 - e * e) / (1.0 + e * cos(trueAnomaly)),&(state->pos));
	pos=pos * (a*(1-e^2)/(1+e*cos(trueAnom)));
% 	else /* hyperbolic orbit */
% 		state->pos = kostMulrv(fabs(elements->a) * (e * e - 1.0) /
% 		(1.0 + e * cos(trueAnomaly)),&(state->pos));
	%TODO

	loc=pos;

%% Calculate Velocity
% 	/* calc velocity vector */
% 	/*  calculate magnitude of velocity vector */
% 	if (e < 1.0) /* elliptical orbit */
% 		{v2 = mu*(2.0/kostAbsv(&(state->pos)) - 1.0/elements->a);}
	v2=u * (2/norm(pos)-1/a);
% 	else /* hyperbolic orbit */
% 		{v2 = mu*(2.0/kostAbsv(&(state->pos)) + 1.0/fabs(elements->a));}


% 	/* calc components of velocity vector perpendicular and parallel to radius vector:
% 	
% 	perpendicular:
% 	vPro = (|h|/|pos|) * normal(h x pos)
% 	parallel:
% 	vO = sqrt(v^2 - |vPro|^2) * sign(sin(trueAnomaly)) * normal(pos)
% 	*/
% 	vPro = kostCrossProductvv(&h, &(state->pos));
	vPro = cross(h,pos);
% 	vPro = kostNormalv(&vPro);
	vPro = vPro/norm(vPro);
% 	vPro = kostMulrv( kostAbsv(&h)/kostAbsv(&(state->pos)) , &vPro);
	vPro = norm(h)/norm(pos).*vPro;
% 
% 	tmpr = sin(trueAnomaly);
	tmpr = sin(trueAnom);
	
% 	if(tmpr == 0.0) /* check for apsis condition to avoid divide by zero */
% 	{
% 		vO = kostConstructv(0.0,0.0,0.0);
% 	}
% 	else
% 	{
% 		kostReal signSinTrueAnomaly = tmpr / fabs(tmpr);
% 
% 		kostReal v0_sq = v2 - kostAbs2v(&vPro);
% 		/* check for small negative numbers resulting from rounding */
% 		if(v0_sq < 0.0) v0_sq = 0.0;
% 
% 		vO = kostNormalv(&(state->pos));
% 		vO = kostMulrv(sqrt(v0_sq)*signSinTrueAnomaly, &vO);
% 	}

	if(tmpr==0)
		v0=[0 0 0];
	else
		v0_sq=v2-norm(vPro)^2;
		if(v0_sq<0)
			v0_sq=0;
		end
		v0=pos/norm(pos);
		v0=sqrt(v0_sq)*sign(tmpr).*v0;
	end

% 	/* add to get velocity vector */
% 	state->vel = kostAddvv(&vPro, &vO);
	vel=vPro+v0;

end

%%
% {
% 	kostReal a;      /*Semi-major axis*/
% 	kostReal e;      /*Eccentricity*/
% 	kostReal i;      /*Inclination*/
% 	kostReal theta;  /*Longitude of ascending node*/
% 	kostReal omegab; /*Longitude of periapsis*/
% 	kostReal L;      /*Mean longitude at epoch*/
% } kostElements;
